#include "../cse452.h"
#include "Camera.h"
#include <cmath>
#include <FL/Fl.H>
#include <float.h>

Camera::Camera() 
{
    setFrom(Point3(3.0, 2.0, 6.0));
    setAt(Point3(0.0, 0.0, 0.0));
    setUp(Vector3(0.0, 1.0, 0.0));
    setZoom(90.0);
    setProjectionCenter( Point3(0,0,0) );
    setSkew(0.0);
    setAspectRatioScale(1.0);
    setNearFar(0.1, 30.0);
}

Camera::~Camera() {
    // destroy your data here
}

// The following three should be unit length and orthogonal to each other
// u vector
Vector3 Camera::getRight() const
{
    return u;
}

// v vector
Vector3 Camera::getUp() const
{
    return v;
}

// - n vector
Vector3 Camera::getLook() const
{
    return -n;
}

double Camera::getSkew() const
{
    // Change this to implement the extra credit
    return skew;
}

double Camera::getAspectRatioScale() const
{
    return aspect;
}

Point3 Camera::getProjectionCenter() const
{
    // Change this to implement the extra credit
    return projectionCenter;
}

Matrix4 Camera::getProjection() const {
    // return the current projection and scale matrix
    return projection;
}

Matrix4 Camera::getWorldToCamera() const {
    // return the current world to camera matrix 
    // Rotation and translation

    return worldToCamera;
}

Matrix4 Camera::getRotationFromXYZ() const
{
    // return just the rotation matrix
    return rotation;
}

Matrix4 Camera::getRotationToXYZ() const
{
    // return just the rotation matrix

    return irotation;
}

Matrix4 Camera::getCameraToWorld() const {
    // return the current camera to world matrix
    // This is the inverse of the rotation, translation, and scale

    return cameraToWorld;
}

int Camera::getWidth()  const{
    // return the current image width

    return width;
}

int Camera::getHeight()  const{
    // return the current image height

	return height;
}

Point3 Camera::getEye()  const{
    // return the current eye location

    return eyePoint;
}

double Camera::getZoom() const
{
    return zoom * M_PI/180.;
}

void Camera::setFrom(const Point3& from) {
    // set the current viewpoint (eye point)
	eyePoint = Point3(from);
	calculateTranslation();
	calculateWorldToCamera();
	calculateCameraToWorld();
}

void Camera::setAt(const Point3& at) {
    // set the point the camera is looking at
    // calling this requires that the from (or eye) point already be valid
	look = at - eyePoint;
	calculateAxis();
	calculateRotation();
	calculateWorldToCamera();
	calculateCameraToWorld();
}

void Camera::setLook(const Vector3& l) {
    // set the direction the camera is looking at
	look = Vector3(l);
	right = look^up;
	calculateAxis();
	calculateRotation();
	calculateWorldToCamera();
	calculateCameraToWorld();
}

void Camera::setUp(const Vector3& u) {
    // set the upwards direction of the camera
	up = Vector3(u);
	right = look^up;
	calculateAxis();
	calculateRotation();
	calculateWorldToCamera();
	calculateCameraToWorld();
}

void Camera::setWidthHeight(int w, int h) {
    // set the current width and height of the film plane
	width = w;
	height = h;
	calculateScaleXYZ();
	calculateProjection();
	calculateCameraToWorld();
}

void Camera::setZoom(double z) {
    // set field of view (specified in degrees)
    zoom = z;
	calculateScaleXY();
	calculateCameraToWorld();
	calculateProjection();
}

void Camera::setNearFar(double n, double f) {
    // set the near and far clipping planes
	nearPlane = n;
	farPlane = f;
	calculateScaleXYZ();
	calculatePerspective();
	calculateCameraToWorld();
	calculateProjection();
}

void Camera::setSkew( double d ) {
	skew = d;
	calculatePerspective();
	calculateProjection();
}

void Camera::setAspectRatioScale( double d ) {
	aspect = (d < 0.) ? -d : d;
	calculatePerspective();
	calculateProjection();
}

void Camera::setProjectionCenter( const Point3 &p ) {
	projectionCenter = Point3(p);
	calculatePerspective();
	calculateProjection();
}


void Camera::moveForward(double dist) {
    // move the camera forward (in the viewing direction)
    // by the amount dist
	eyePoint = eyePoint + dist * -n;
	calculateTranslation();
	calculateWorldToCamera();
	calculateCameraToWorld();
}

void Camera::moveSideways(double dist) {
    // move the camera sideways (orthogonal to the viewing direction)
    // by the amount dist
	eyePoint = eyePoint + dist * u;
	calculateTranslation();
	calculateWorldToCamera();
	calculateCameraToWorld();
}

void Camera::moveVertical(double dist) {
    // move the camera vertically (along the up vector)
    // by the amount dist
	eyePoint = eyePoint + dist * v;
	calculateTranslation();
	calculateWorldToCamera();
	calculateCameraToWorld();
}

void Camera::rotateYaw(double angle) {
    // rotate the camera left/right (around the up vector)
	Matrix4 r = Matrix4::rotation(v, angle);
	look = r*look;
	right = look^up;
	calculateAxis();
	calculateRotation();
    calculateWorldToCamera();
	calculateCameraToWorld();
}

void Camera::rotatePitch(double angle) {
    // rotate the camera up/down (pitch angle)
	Matrix4 r = Matrix4::rotation(u, angle);
	look = r*look;
	up = r*up;
	calculateAxis();
	calculateRotation();
	calculateWorldToCamera();
	calculateCameraToWorld();
}


//Note: Does not maintain original up look and right vectors
void Camera::rotateAroundAtPoint(int axis, double angle, double focusDist) {
    // Rotate the camera around the right (0), up (1), or look (2) vector
    // around the point at eye + look * focusDist

	up = v;
	right = u;
	look = -n;

	Point3 focusPoint = eyePoint - n * focusDist;

	Matrix4 m = Matrix4::translation(focusPoint);
	if(axis == 0)
		m = Matrix4::rotation(u, angle) * m;
	else if(axis == 1)
		m = Matrix4::rotation(v, angle) * m;
	else
		m = Matrix4::rotation(n, angle) * m;
	m = Matrix4::translation(-focusPoint) * m;

	eyePoint = m * eyePoint;
	up = m * up;
	look = m * look;
	right = look^up;
	calculateAxis();
	eyePoint = (focusPoint + n * focusDist);

	calculateRotation();
	calculateTranslation();
	calculateWorldToCamera();
	calculateCameraToWorld();
}


void Camera::moveKeyboard( ) {
    // you may change key controls for the interactive
    // camera controls here, make sure you document your changes
    // in your README file

    if (Fl::event_key('w'))
        moveForward(+0.05);
    if (Fl::event_key('s'))
        moveForward(-0.05);
    if (Fl::event_key('a'))
        moveSideways(-0.05);
    if (Fl::event_key('d'))
        moveSideways(+0.05);
    if (Fl::event_key(FL_Up))
        moveVertical(+0.05);
    if (Fl::event_key(FL_Down))
        moveVertical(-0.05);
    if (Fl::event_key(FL_Left))
        rotateYaw(+0.05);
    if (Fl::event_key(FL_Right))
        rotateYaw(-0.05);
    if (Fl::event_key(FL_Page_Up))
        rotatePitch(+0.05);
    if (Fl::event_key(FL_Page_Down))
        rotatePitch(-0.05);
}

void Camera::calculateAxis() {
	n = -look / look.length();
	v = up - n*(up*n);
	v.normalize();
	u = v^n;
}
void Camera::calculateTranslation() {
	translation = Matrix4(
		Vector4(1., 0., 0., -eyePoint[0]),
		Vector4(0., 1., 0., -eyePoint[1]),
		Vector4(0., 0., 1., -eyePoint[2]),
		Vector4(0., 0., 0., 1.));
	itranslation = Matrix4(
		Vector4(1., 0., 0., eyePoint[0]),
		Vector4(0., 1., 0., eyePoint[1]),
		Vector4(0., 0., 1., eyePoint[2]),
		Vector4(0., 0., 0., 1.));
	#ifdef _CAMERA_DEBUG
		if(!(Matrix4::identity().approxEqual(translation * itranslation)))
			std::cout << "Translation Incorrect" << std::endl;
	#endif
}
void Camera::calculateRotation() {
	rotation = Matrix4(
		Vector4(u[0], u[1], u[2], 0),
		Vector4(v[0], v[1], v[2], 0),
		Vector4(n[0], n[1], n[2], 0),
		Vector4(0., 0., 0., 1.));
	irotation = Matrix4(
		Vector4(u[0], v[0], n[0], 0),
		Vector4(u[1], v[1], n[1], 0),
		Vector4(u[2], v[2], n[2], 0),
		Vector4(0., 0., 0., 1.));
	#ifdef _CAMERA_DEBUG
		if(!(Matrix4::identity().approxEqual(rotation * irotation))){
			std::cout << "Rotation Incorrect" << std::endl;
			rotation.print();
			irotation.print();
			(rotation * irotation).print();
		}
	#endif
}

void Camera::calculateScaleXY() {
	scaleXY = Matrix4(
		Vector4( 1./(std::tan(zoom*M_PI/360.)), 0., 0., 0.),
		Vector4( 0., 1./std::tan(zoom*M_PI/360.), 0., 0.),
		Vector4( 0., 0., 1., 0.),
		Vector4( 0., 0., 0., 1.));
	iscaleXY = Matrix4(
		Vector4( std::tan(zoom*M_PI/360.), 0., 0., 0.),
		Vector4( 0., std::tan(zoom*M_PI/360.), 0., 0.),
		Vector4( 0., 0., 1., 0.),
		Vector4( 0., 0., 0., 1.));
	#ifdef _CAMERA_DEBUG
		if(!(Matrix4::identity().approxEqual(scaleXY * iscaleXY))){
			std::cout << "ScaleXY Incorrect" << std::endl;
			scaleXY.print();
			iscaleXY.print();
			(scaleXY * iscaleXY).print();
		}
	#endif
}
void Camera::calculateScaleXYZ() {
	scaleXYZ = Matrix4(
		Vector4(1./(farPlane), 0., 0., 0.),
		Vector4(0., static_cast<double>(width)/(height*farPlane), 0., 0.),
		Vector4(0., 0., 1./farPlane, 0.),
		Vector4(0, 0., 0., 1.));
	iscaleXYZ = Matrix4(
		Vector4(farPlane, 0., 0., 0.),
		Vector4(0., (farPlane*static_cast<double>(height))/static_cast<double>(width), 0., 0.),
		Vector4(0., 0., farPlane, 0.),
		Vector4(0, 0., 0., 1.));
	#ifdef _CAMERA_DEBUG
		if(!(Matrix4::identity().approxEqual(scaleXYZ * iscaleXYZ))){
			std::cout << "ScaleXYZ Incorrect" << std::endl;
			scaleXYZ.print();
			iscaleXYZ.print();
			(scaleXYZ * iscaleXYZ).print();
		}
	#endif
}
void Camera::calculatePerspective() {
	perspective = Matrix4(
		Vector4(aspect, skew, projectionCenter[0], 0.),
		Vector4(0., 1., projectionCenter[1], 0.),
		Vector4(0., 0., 1./(nearPlane/farPlane - 1.), (nearPlane/farPlane)/(nearPlane/farPlane - 1.)),
		Vector4(0., 0., -1., 0.));
	iperspective = Matrix4(
		Vector4(1./aspect, -skew/aspect, 0., (projectionCenter[0]-skew*projectionCenter[1])/aspect),
		Vector4(0., 1., 0., projectionCenter[1]),
		Vector4(0., 0., 0., -1.),
		Vector4(0., 0., (nearPlane/farPlane - 1.)/(nearPlane/farPlane), 1./(nearPlane/farPlane)));
	#ifdef _CAMERA_DEBUG
		if(!(Matrix4::identity().approxEqual(perspective * iperspective))){
			std::cout << "Perspective Incorrect" << std::endl;
			perspective.print();
			iperspective.print();
			(perspective * iperspective).print();
		}
	#endif
}
void Camera::calculateWorldToCamera() {
	worldToCamera = rotation * translation;
}
void Camera::calculateCameraToWorld() {
	cameraToWorld = itranslation * irotation * iscaleXY * iscaleXYZ;
}
void Camera::calculateProjection() {
	projection = perspective * scaleXYZ * scaleXY;
}
	

